/***************************************************************************
				CompositeBody.cpp
                               -------------------
Copyright (c) 2009-2010 
Author: Kishor D. Bhalerao (kishor8dm@gmail.com)
Contributors: 

This file is associated with the Ph.D. dissertation,"ON METHODS FOR EFFICIENT
AND ACCURATE DESIGN AND SIMULATION OF MULTIBODY SYSTEMS", Bhalerao K.D,
Rensselaer Polytechnic Institute, 2010.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

1.) Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.

2.) Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation and/or
other materials provided with the distribution.

3.) The name of the authors, their employers, or sponsoring organizations may
not be used to endorse or promote products derived from this software without
specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO
EVENT SHALL THE AUTHORS, THEIR EMPLOYERS, OR SPONSORING ORGANIZATIONS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
***************************************************************************/

#include "CompositeBody.h"
#include "MatOps.h"
#include "DcaDataTypes.h"
#include "HandleEqs.h"

// This needs to be changed.
CompositeBody::CompositeBody(){
	
	A_=B_=DT_.psi11=DT_.psi12=DT_.psi21=DT_.psi22=0;
	C_=DT_.psi13=DT_.psi23=0;Parent_=Child_=CompositeBodyId_=0;
}

void CompositeBody::expressEomDatainBasis(EomData& H, Mat33&B_C_A){
	MatOp op; Mat33 Z(0);
	Mat66 B_C_A_sp = op.toMat66(B_C_A,Z,Z,B_C_A);
	const EomData& DT = this->getEomData();

	H.psi11 = B_C_A_sp*DT.psi11*(~B_C_A_sp);
	H.psi12 = B_C_A_sp*DT.psi12*(~B_C_A_sp);
	H.psi21 = B_C_A_sp*DT.psi21*(~B_C_A_sp);
	H.psi22 = B_C_A_sp*DT.psi22*(~B_C_A_sp);
	H.psi13 = B_C_A_sp*DT.psi13;
	H.psi23 = B_C_A_sp*DT.psi23;
}

void CompositeBody::expressEomDatainBasis(EomData& H,const CompositeBody& body, Mat33& B_C_A){
	MatOp op; Mat33 Z(0);
	Mat66 B_C_A_sp = op.toMat66(B_C_A,Z,Z,B_C_A);
	const EomData& DT = body.getEomData();

	H.psi11 = B_C_A_sp*DT.psi11*(~B_C_A_sp);
	H.psi12 = B_C_A_sp*DT.psi12*(~B_C_A_sp);
	H.psi21 = B_C_A_sp*DT.psi21*(~B_C_A_sp);
	H.psi22 = B_C_A_sp*DT.psi22*(~B_C_A_sp);
	H.psi13 = B_C_A_sp*DT.psi13;
	H.psi23 = B_C_A_sp*DT.psi23;
}

void CompositeBody::expressEomDatainBasis(EomData& H,const DcaBodyData& body, const Vec6& psi13, const Vec6& psi23, Mat33& B_C_A){
	MatOp op; Mat33 Z(0);
	Mat66 B_C_A_sp = op.toMat66(B_C_A,Z,Z,B_C_A);

	H.psi11 = B_C_A_sp*body.psi11*(~B_C_A_sp);
	H.psi12 = B_C_A_sp*body.psi12*(~B_C_A_sp);
	H.psi21 = B_C_A_sp*body.psi21*(~B_C_A_sp);
	H.psi22 = B_C_A_sp*body.psi22*(~B_C_A_sp);

	H.psi13 = B_C_A_sp*psi13;
	H.psi23 = B_C_A_sp*psi23;
}

// Calculate X12 for different joints. This function is written to side step the 
// problem of using a joint Motion map matrix of different dimensions for each 
// type of joint. 

void CompositeBody::calcX12(const EomData& b1,const DcaBodyData& b2,const JointType& JT,Mat66& X12){
	MatOp op;
	switch(JT)
	{
	case REVOLUTEJOINT:
		{
			Mat65 D(0);
			D(0,0)=D(1,1)=D(3,2)=D(4,3)=D(5,4)=1.0;
			Matrix iE12Mat;
			Mat55 E12, iE12;
			E12 = (~D)*(b1.psi22 + b2.psi11)*(D);
			Matrix E12Mat(5,5);
			op.toMatrix(E12, E12Mat);

			SimTK::FactorLU lu(E12Mat);
			lu.inverse(iE12Mat);
			iE12 = op.toMat55(iE12Mat);
			X12 = (D)*iE12*(~D);
			//std::cout << "7" << std::endl;
			break;
		}
	case SPHERICALJOINT:
		{
			Mat63 D(0);
			D(3,0)=D(4,1)=D(5,2)=1.0;
			Matrix iE12Mat;
			Mat33 E12, iE12;
			E12 = (~D)*(b1.psi22 + b2.psi11)*(D);
			Matrix E12Mat = op.toMatrix(E12);
			SimTK::FactorLU lu(E12Mat);
			lu.inverse(iE12Mat);
			iE12 = op.toMat33(iE12Mat);
			X12 = (D)*iE12*(~D);
			break;
		}
	case RIGIDJOINT:
		{
			Matrix iE12Mat;
			Mat66 tmpMat66 = b1.psi22 + b2.psi11;			
			Matrix E12Mat = op.toMatrix(tmpMat66);			
			SimTK::FactorLU lu(E12Mat);
			lu.inverse(iE12Mat);
			op.toMat66(iE12Mat,X12);
			break;
		}
	}
}

void CompositeBody::calcX12_ProjectMom(const DcaBodyData_projectMom& b1, const DcaBodyData& b2,const JointType& JT, Mat66& X12){
	MatOp op;
	switch(JT){
		// Regular Joint here is revolute joint, other joint types are not allowed
		// for the moment, 
		case REVOLUTEJOINT:
		{
			Mat65 D(0);
			D(0,0)=D(1,1)=D(3,2)=D(4,3)=D(5,4)=1.0;
			Matrix iE12Mat;
			Mat55 E12, iE12;
			E12 = (~D)*(b1.psi22 + b2.psi11)*(D);
			Matrix E12Mat(5,5);
			op.toMatrix(E12, E12Mat);
			SimTK::FactorLU lu(E12Mat);
			lu.inverse(iE12Mat);
			iE12 = op.toMat55(iE12Mat);
			X12 = (D)*iE12*(~D); 
			break;
		}
		
		case RIGIDJOINT:
		{
			Matrix iE12Mat;
			Mat66 tmpMat66 = b1.psi22 + b2.psi11;			
			Matrix E12Mat = op.toMatrix(tmpMat66);			
			SimTK::FactorLU lu(E12Mat);
			lu.inverse(iE12Mat);
			op.toMat66(iE12Mat,X12);			
			break;
		}
	}
}

void CompositeBody::calcX12_ProjectMom(const EomData& b1, const DcaBodyData& b2,const JointType& JT, Mat66& X12){
	MatOp op;
	switch(JT){
		// Regular Joint here is revolute joint, other joint types are not allowed
		// for the moment.
		case REVOLUTEJOINT:
		{
			Mat65 D(0);
			D(0,0)=D(1,1)=D(3,2)=D(4,3)=D(5,4)=1.0;
			Matrix iE12Mat;
			Mat55 E12, iE12;
			E12 = (~D)*(b1.psi22 + b2.psi11)*(D);
			Matrix E12Mat(5,5);
			op.toMatrix(E12, E12Mat);
			SimTK::FactorLU lu(E12Mat);
			lu.inverse(iE12Mat);
			iE12 = op.toMat55(iE12Mat);
			X12 = (D)*iE12*(~D); 
			break;
		}
		
		case RIGIDJOINT:
		{
			Matrix iE12Mat;
			Mat66 tmpMat66 = b1.psi22 + b2.psi11;			
			Matrix E12Mat = op.toMatrix(tmpMat66);			
			SimTK::FactorLU lu(E12Mat);
			lu.inverse(iE12Mat);
			op.toMat66(iE12Mat,X12);			
			break;
		}
	}
}

// See Simbody Theory Manual Page 41 Figure 4 for the definitions of frames F,M
void CompositeBody::FormCompositeBody(const int& body1id, const int& body2id,const int& CompositeId,
				      SimbodyMatterSubsystem& matter, State& state, TwoHandleEquations& THE,
					  const int **FreeJoints){
	
	// Define op to perform some Matrix operations
	MatOp op;

	// Set Various Indices
	// Body Ids correspond to the MobilzedBodyIndex
	CompositeBodyId_ = CompositeId;
	Parent_ = body1id; // Body 1 is parent, the basis B1 is equivalent to basis P
	Child_ = body2id; // Body 2 is child, the basis B2 is equivalent to basis B

	const DcaBodyData *Mbs = THE.DcaObj.getSystemPtr(); // returns body which contains all phi's and geometries in mobilizer frame
	
	const DcaBodyData& body1 = Mbs[Parent_ -1]; // Child_ = Parent_ + 1
	const DcaBodyData& body2 = Mbs[Child_ -1];
	
	const Vec6 *psi13 = THE.Eqns.getpsi13();
	const Vec6 *psi23 = THE.Eqns.getpsi23();
	
	const Vec6& b1psi13 = psi13[Parent_-1];
	const Vec6& b1psi23 = psi23[Parent_-1];

	const Vec6& b2psi13 = psi13[Child_-1];
	const Vec6& b2psi23 = psi23[Child_-1];

	const Vec3 *N_W_K = THE.Eqns.getN_W_K();
	const Vec3& b1N_W_K = N_W_K[Parent_-1];
	const Vec3& b2N_W_K = N_W_K[Child_-1];

	// Get Mobilized bodies
	//MobilizedBody Body1, Body2;
	const MobilizedBody& Body1 = matter.getMobilizedBody(MobilizedBodyIndex(Parent_));
	const MobilizedBody& Body2 = matter.getMobilizedBody(MobilizedBodyIndex(Child_));

	Mat33 F_C_B1 = ~Body2.getInboardFrame(state).R();
	Mat33 B1_C_Mp = Body1.getOutboardFrame(state).R();
	Mat33 F_C_M = Body2.getMobilizerTransform(state).R();
	Mat33 M_C_B1 = (~F_C_M)*F_C_B1;
	Mat33 M_C_Mp = M_C_B1*B1_C_Mp; // M: mobilizaer of the child, Mp: mobilizaer of the parent

	// All Matrices of b1 are in M basis
	EomData b1;
	expressEomDatainBasis(b1,body1,b1psi13,b1psi23,M_C_Mp); // Tranform all the coeff of parent from its own M frame to 
	// the M frame of the child
	
	JointType JT;
	Mat66 X12;
	//matter.getRep().getRigidBodyNode(Child_).getDOF()
	/*switch(1){
		case 3:{JT=SPHERICALJOINT;break;}
		case 1:{JT=REVOLUTEJOINT;break;}
		case 0:{JT=RIGIDJOINT;break;}
		case 6:{JT=FREEJOINT;break;}
	}*/

	if((*FreeJoints)[Parent_]==-1){
		JT = RIGIDJOINT;		
	}
	else if((*FreeJoints)[Parent_]==1){
		JT = REVOLUTEJOINT;
	}
	else{
		cout<<"\nCompositeBody.h:\n";
		char ch;
		cin>>ch;
	}

	CompositeBody::calcX12(b1,body2,JT,X12);	

	
	// To calculate Constraint force between bodies k and k+1 as a linear
	// combination of force acting on boundary handles
	// F^{k+1}_1 = A* F^{k}_1 + B* F^{k+1}_2 + C
	this->A_ = X12*b1.psi21;
	this->B_ = -X12*body2.psi12;
	
	Vec3 wx = (M_C_Mp*b1N_W_K)%b2N_W_K;
	//Vec3 wx = ((M_C_N)*(N_W_Body1 % N_W_Body2));
	Vec3 tmpVec(0,0,0);
	this->C_ = X12*(b1.psi23-b2psi13+op.toVec6(wx,tmpVec));

	this->DT_.psi11 = b1.psi11 - b1.psi12*(this->A_);
	this->DT_.psi12 = -b1.psi12*(this->B_);
	this->DT_.psi13 = b1.psi13-b1.psi12*(this->C_);

	this->DT_.psi21 = body2.psi21*(this->A_);
	this->DT_.psi22 = body2.psi21*(this->B_) + body2.psi22;
	this->DT_.psi23 = body2.psi21*(this->C_) + b2psi23;
}

void CompositeBody::FormCompositeBody(CompositeBody& cBody,const int& ChildId,const int& CompositeId,
				      SimbodyMatterSubsystem& matter, State& state, TwoHandleEquations& THE,
				      const int **FreeJoints){
	MatOp op;
	
	this->CompositeBodyId_ = CompositeId;
	this->Parent_ = cBody.getCompositeBodyId();
	this->Child_ = ChildId;

	const DcaBodyData *Mbs = THE.DcaObj.getSystemPtr();
	const DcaBodyData& body2 = Mbs[Child_ -1];

	const Vec6 *psi13 = THE.Eqns.getpsi13();
	const Vec6 *psi23 = THE.Eqns.getpsi23();

	const Vec6& b2psi13 = psi13[Child_-1];
	const Vec6& b2psi23 = psi23[Child_-1];

	const Vec3 *N_W_K = THE.Eqns.getN_W_K();
	const Vec3& b1N_W_K = N_W_K[cBody.getChild()-1];
	const Vec3& b2N_W_K = N_W_K[Child_-1];

	// Get Mobilized bodies
	// MobilizedBody Body1, Body2;
	// At the current joint, Body 1 is parent, Body 2 is child
	const MobilizedBody& mBody1 = matter.getMobilizedBody(MobilizedBodyIndex(cBody.getChild()));
	const MobilizedBody& mBody2 = matter.getMobilizedBody(MobilizedBodyIndex(Child_));

	Mat33 F_C_B1 = ~mBody2.getInboardFrame(state).R();
	Mat33 B1_C_Mp = mBody1.getOutboardFrame(state).R();
	Mat33 F_C_M = mBody2.getMobilizerTransform(state).R();
	Mat33 M_C_B1 = (~F_C_M)*F_C_B1;
	Mat33 M_C_Mp = M_C_B1*B1_C_Mp;

	EomData b1;
	expressEomDatainBasis(b1,cBody,M_C_Mp);

	JointType JT;
	Mat66 X12;
		
    //std::cout << (*FreeJoints) << std::endl;
	//std::cout << (*FreeJoints)[cBody.getChild()] << std::endl;

	if((*FreeJoints)[cBody.getChild()]==-1){
		JT = RIGIDJOINT;		
	}
	else if((*FreeJoints)[cBody.getChild()]==1){
		JT = REVOLUTEJOINT;
	}
	else{
		cout<<"\nCompositeBody.h:\n";
		char ch;
		cin>>ch;
	}
	/*
// 	switch(isRigid){
// 		case 3:{JT=SPHERICALJOINT;break;}
// 		case 0:{JT=REVOLUTEJOINT;break;}
// 		case 1:{JT=RIGIDJOINT;break;}
// 		case 6:{JT=FREEJOINT;break;}
// 	}*/
	CompositeBody::calcX12(b1,body2,JT,X12);

	// To calculate Constraint force between bodies k and k+1 as a linear
	// combination of force acting on boundary handles
	// F^{k+1}_1 = A* F^{k}_1 + B* F^{k+1}_2 + C
	this->A_ = X12*b1.psi21;
	this->B_ = -X12*body2.psi12;
	
	Vec3 wx = (M_C_Mp*b1N_W_K)%b2N_W_K;
	//Vec3 wx = ((M_C_N)*(N_W_Body1 % N_W_Body2));
	Vec3 tmpVec(0,0,0);
	this->C_ = X12*(b1.psi23-b2psi13+op.toVec6(wx,tmpVec));

	this->DT_.psi11 = b1.psi11 - b1.psi12*(this->A_);
	this->DT_.psi12 = -b1.psi12*(this->B_);
	this->DT_.psi13 = b1.psi13-b1.psi12*(this->C_);

	this->DT_.psi21 = body2.psi21*(this->A_);
	this->DT_.psi22 = body2.psi21*(this->B_) + body2.psi22;
	this->DT_.psi23 = body2.psi21*(this->C_) + b2psi23;
	
}

void CompositeBody::formCompositeBody_projectMom(const int& body1id, const int& body2id, const int& CompositeId, 
						 SimbodyMatterSubsystem& matter, State& state,const DcaPreprocessor& DcaObj,
						 int **FreeJoints){
	// Define op to perform some Matrix operations
	MatOp op;

	// Set Various Indices
	// Body Ids correspond to the MobilzedBodyIndex
	CompositeBodyId_ = CompositeId;
	Parent_ = body1id; // Body 1 is parent, the basis B1 is equivalent to basis P
	Child_ = body2id; // Body 2 is child, the basis B2 is equivalent to basis B

	const DcaBodyData *Mbs = DcaObj.getSystemPtr();
	
	const DcaBodyData& body1 = Mbs[Parent_ -1]; // Child_ = Parent_ + 1
	const DcaBodyData& body2 = Mbs[Child_ -1];
	
	// Get Mobilized bodies
	//MobilizedBody Body1, Body2;
	const MobilizedBody& Body1 = matter.getMobilizedBody(MobilizedBodyIndex(Parent_));
	const MobilizedBody& Body2 = matter.getMobilizedBody(MobilizedBodyIndex(Child_));

	Mat33 F_C_B1 = ~Body2.getInboardFrame(state).R();
	Mat33 B1_C_Mp = Body1.getOutboardFrame(state).R();
	Mat33 F_C_M = Body2.getMobilizerTransform(state).R();
	Mat33 M_C_B1 = (~F_C_M)*F_C_B1;
	Mat33 M_C_Mp = M_C_B1*B1_C_Mp;

	// All Matrices of b1 are in M basis	
	//expressEomDatainBasis(b1,body1,b1psi13,b1psi23,M_C_Mp);
	const Mat66& psi11 = body1.psi11;
	const Mat66& psi12 = body1.psi12;
	
	const Mat66& psi21 = body1.psi21;
	const Mat66& psi22 = body1.psi22;
	
	DcaBodyData_projectMom b1;
	
	op.expressMat66inBasis(b1.psi11,psi11,M_C_Mp);
	op.expressMat66inBasis(b1.psi12,psi12,M_C_Mp);
	op.expressMat66inBasis(b1.psi21,psi21,M_C_Mp);
	op.expressMat66inBasis(b1.psi22,psi22,M_C_Mp);
	
	Mat66 X12;
	JointType JT;
	Vec6 Vt(0);
	
	

	if((*FreeJoints)[body1id]==0){
		JT = RIGIDJOINT;
		Vt(2)=-Body2.getUAsVector(state)[0];
	}
	else if((*FreeJoints)[body1id]==-1){
		JT = RIGIDJOINT;
	}
	else if((*FreeJoints)[body1id]==1){
		JT = REVOLUTEJOINT;
	}
	
	/*// this part has to be a little more elaborate
	if(JointTransition){
		JT = RIGIDJOINT;
	}
	else{
		JT = REVOLUTEJOINT;		
	}*/	
	
	calcX12_ProjectMom(b1, body2, JT, X12);
	
	// To calculate Constraint Impulse between bodies k and k+1 as a linear
	// combination of impulse acting on boundary handles
	// I^{k+1}_1 = A* I^{k}_1 + B* I^{k+1}_2 + C
	this->A_ = X12*b1.psi21;
	this->B_ = -X12*body2.psi12;
	this->C_ = X12*Vt;
	
	// note: For this particular case, there is no psi13 or psi23 in R.H.S
	// These will appear when combining a body and a composite body
	
	this->DT_.psi11 = b1.psi11 - b1.psi12*(this->A_);
	this->DT_.psi12 = -b1.psi12*(this->B_);
	this->DT_.psi13 = -b1.psi12*(this->C_);

	this->DT_.psi21 = body2.psi21*(this->A_);
	this->DT_.psi22 = body2.psi21*(this->B_) + body2.psi22;
	this->DT_.psi23 = body2.psi21*(this->C_);	
}


void CompositeBody::formCompositeBody_projectMom(CompositeBody& cBody, const int& ChildId, const int& CompositeId, 
						 SimbodyMatterSubsystem& matter, State& state,const DcaPreprocessor& DcaObj,
						 int **FreeJoints){
	//MatOp op;
	
	this->CompositeBodyId_ = CompositeId;
	this->Parent_ = cBody.getCompositeBodyId();
	this->Child_ = ChildId;

	const DcaBodyData *Mbs = DcaObj.getSystemPtr();
	const DcaBodyData& body2 = Mbs[Child_ -1];
	
	// Get Mobilized bodies
	// MobilizedBody Body1, Body2;
	// At the current joint, Body 1 is parent, Body 2 is child
	const MobilizedBody& mBody1 = matter.getMobilizedBody(MobilizedBodyIndex(cBody.getChild()));
	const MobilizedBody& mBody2 = matter.getMobilizedBody(MobilizedBodyIndex(Child_));

	Mat33 F_C_B1 = ~mBody2.getInboardFrame(state).R();
	Mat33 B1_C_Mp = mBody1.getOutboardFrame(state).R();
	Mat33 F_C_M = mBody2.getMobilizerTransform(state).R();
	Mat33 M_C_B1 = (~F_C_M)*F_C_B1;
	Mat33 M_C_Mp = M_C_B1*B1_C_Mp;

	EomData b1;
	expressEomDatainBasis(b1,cBody,M_C_Mp);
	
	Mat66 X12;
	JointType JT;
	Vec6 Vt(0);
	
// 	if((*FreeJoints)[mBody1.getMobilizedBodyIndex()]==0){
// 		JT = RIGIDJOINT;
// 		Vt(2)=-mBody2.getUAsVector(state)[0];		
// 	}
// 	else{
// 		JT = REVOLUTEJOINT;		
// 	}

	if((*FreeJoints)[mBody1.getMobilizedBodyIndex()]==0){
		JT = RIGIDJOINT;
		Vt(2)=-mBody2.getUAsVector(state)[0];
	}
	else if((*FreeJoints)[mBody1.getMobilizedBodyIndex()]==-1){
		JT = RIGIDJOINT;
	}
	else if((*FreeJoints)[mBody1.getMobilizedBodyIndex()]==1){
		JT = REVOLUTEJOINT;
	}

	CompositeBody::calcX12_ProjectMom(b1,body2,JT,X12);
	
	// To calculate Constraint Impulse between bodies k and k+1 as a linear
	// combination of impulse acting on boundary handles
	// I^{k+1}_1 = A* I^{k}_1 + B* I^{k+1}_2 + C
	this->A_ = X12*b1.psi21;
	this->B_ = -X12*body2.psi12;	
	this->C_ = X12*(Vt+b1.psi23);
	
	// In this particular case, CompositeBody 1 has a psi13 and psi23 term
	// This is not the case for body2
	
	this->DT_.psi11 = b1.psi11 - b1.psi12*(this->A_);
	this->DT_.psi12 = -b1.psi12*(this->B_);
	this->DT_.psi13 = b1.psi13-b1.psi12*(this->C_);

	this->DT_.psi21 = body2.psi21*(this->A_);
	this->DT_.psi22 = body2.psi21*(this->B_) + body2.psi22;
	this->DT_.psi23 = body2.psi21*(this->C_);		
}
